

#ifndef __CE_MATH_QUATERNION_H__
#define __CE_MATH_QUATERNION_H__

#include <math.h>
#include <math/vector.h>
#include <math/matrix.h>

class ceQuaternion
{
public:
	ceVector3f	v;
	float				s;

public:
	inline ceQuaternion (float x=0.0f, float y=0.0f, float z=0.0f, float w=1.0f) : v (x, y, z), s (w) {  } 
	inline ceQuaternion (const ceVector3f v, float s) : v(v), s(s) { }
  inline ceQuaternion (const ceMatrix4f& mat) { Set (mat); }

  inline void Set (const ceMatrix4f& mat)
  {
    s = (float)sqrt (mat._00 + mat._11 + mat._22 + 1.0f) / 2.0f;
    if (s > 1.0f) s = 1.0f;
    float s4 = 4 * s;
    v.x = (mat._21 - mat._12) / s4;
    v.y = (mat._02 - mat._20) / s4;
    v.z = (mat._10 - mat._01) / s4;
  }

	inline void SetAxisAngle (const ceVector3f& axis, float angle)
	{
		float hangle = angle / 2.0f;
		v = axis * sin (hangle);
		s = cos (hangle);
	}

	inline float GetAngle () const
	{
		return 2.0f * acos (s);
	}

	inline ceVector3f GetAxis () const
	{
		float angle = sin(acos (s));
    if (-0.0000001f < angle && angle < 0.0000001f)
			{
				return ceVector3f (0.0f, 0.0f, 0.0f);
			}

    return v / angle;
	}


  inline friend ceQuaternion operator* (const ceQuaternion& q0, const ceQuaternion& q1)
  {
    return ceQuaternion (
          q0.s*q1.v + q1.s*q0.v + q0.v%q1.v,
          q0.s*q1.s - q0.v.Dot(q1.v)
          );
  }

  inline friend ceQuaternion operator* (const ceVector3f& v, const ceQuaternion& q)
  {
    return ceQuaternion (
          v*q.s + v%q.v,
          -v.Dot(q.v)
          );
  }

  inline friend ceQuaternion operator* (const ceQuaternion& q, const ceVector3f& v)
  {
    return ceQuaternion (
          q.s*v + q.v%v,
          -q.v.Dot(v)
          );
  }

  inline ceQuaternion operator* (const ceVector3f& other)
  {
    return ceQuaternion (
          other*s + v % other,
          -v.Dot(other));
  }

  inline ceVector3f Rotate (const ceVector3f& vector)
  {
    return (*this * vector * Conjugated()).v;
  }

  inline ceQuaternion Conjugated () const
  {
    return ceQuaternion (-v, s);
  }

  inline const ceQuaternion& Conjugate ()
  {
    v = -v;
    return *this;
  }

  inline float Length () const
  {
    return sqrt (Dot ());
  }

  inline float Dot () const
  {
    return s*s + v.Dot();
  }

  inline float Dot (const ceQuaternion& other) const
  {
    return s*other.s + v.Dot(other.v);
  }


  inline float Angle (const ceQuaternion& other) const
  {
    return (float)acos (Dot(other));
  }


};


#endif /* ! __CE_MATH_QUATERNION_H__ */
